#include "IBC_nav.h"

IBCnav_FUSION_T G_IBCnav_FusionData_t = {0};
IBCnav_ORIGIN_T G_IBCnav_OriginData_t = {0};

int L_IBCnav_Step_i = 0;
uint16_t L_IBCnav_TempHeader_u16 = 0;
uint16_t L_IBCnav_TempLen_u16 = 0;
uint16_t L_IBCnav_TempLoc_u16 = 0;
uint8_t L_IBCnav_TempBuf_u8g[IBCnav_MAX_LEN] = {0};

void IBCnav_prfOriginData(int (*println)(const char *fmt, ...));
void IBCnav_prfFusionData(int (*println)(const char *fmt, ...));

uint16_t IBCnav_checksum(uint8_t *data, uint8_t len)
{
    uint16_t sum = 0;
    for (uint8_t i = 0; i < len; i++)
    {
        sum += data[i];
    }
    return sum;
}

int IBCnav_init(void)
{
    memset(&G_IBCnav_FusionData_t, 0, sizeof(G_IBCnav_FusionData_t));
    memset(&G_IBCnav_OriginData_t, 0, sizeof(G_IBCnav_OriginData_t));

    return 0;
}

int IBCnav_pushData(const uint8_t data)
{
    int ret = 0;
    switch (L_IBCnav_Step_i)
    {
    case 0:
        // 找到HEAD
        L_IBCnav_TempHeader_u16 = L_IBCnav_TempHeader_u16 >> 8;
        L_IBCnav_TempHeader_u16 |= (data << 8);
        if (L_IBCnav_TempHeader_u16 == IBCnav_HEAD)
        {
            L_IBCnav_Step_i = 1;
            L_IBCnav_TempBuf_u8g[0] = L_IBCnav_TempHeader_u16 & 0xFF;
            L_IBCnav_TempBuf_u8g[1] = data;

            L_IBCnav_TempHeader_u16 = 0;
        }
        break;
    case 1:
        // 获取LEN
        L_IBCnav_TempLen_u16 = data;
        L_IBCnav_TempBuf_u8g[2] = data;
        if (L_IBCnav_TempLen_u16 < IBCnav_MAX_LEN)
        {
            L_IBCnav_TempLoc_u16 = 3;
            L_IBCnav_Step_i = 2;
        }
        else
        {
            L_IBCnav_Step_i = 0;
        }
        break;
    case 2:
        // 获取LEN长度的DATA数据
        if (L_IBCnav_TempLoc_u16 < (L_IBCnav_TempLen_u16 - 1))
        {
            L_IBCnav_TempBuf_u8g[L_IBCnav_TempLoc_u16] = data;
            L_IBCnav_TempLoc_u16++;
        }
        else
        {
            L_IBCnav_TempBuf_u8g[L_IBCnav_TempLoc_u16] = data;
            L_IBCnav_TempLoc_u16++;

            // 校验检查
            if (((L_IBCnav_TempBuf_u8g[L_IBCnav_TempLen_u16 - 1] << 8) | L_IBCnav_TempBuf_u8g[L_IBCnav_TempLen_u16 - 2]) == IBCnav_checksum(L_IBCnav_TempBuf_u8g, L_IBCnav_TempLen_u16 - 2))
            {
                // 数据赋值
                switch (L_IBCnav_TempBuf_u8g[3])
                {
                case IBCnav_FUSION_ID:
                    memcpy(&G_IBCnav_FusionData_t, &L_IBCnav_TempBuf_u8g[0], sizeof(G_IBCnav_FusionData_t));
                    break;
                case IBCnav_ORIGIN_ID:
                    memcpy(&G_IBCnav_OriginData_t, &L_IBCnav_TempBuf_u8g[0], sizeof(G_IBCnav_OriginData_t));
                    break;
                default:
                    break;
                }
                ret = 1;
            }

            L_IBCnav_TempLoc_u16 = 0;
            L_IBCnav_Step_i = 0;
        }

        break;
    default:
        break;
    }
    return ret;
}

void IBCnav_prfOriginData(int (*println)(const char *fmt, ...))
{
    println("\n\nOriginData:\n");
    println("head: 0x%04x\n", G_IBCnav_OriginData_t.head);
    println("len: %d\n", G_IBCnav_OriginData_t.len);
    println("id: %d\n", G_IBCnav_OriginData_t.id);
    println("count: %d\n", G_IBCnav_OriginData_t.count);

    println("DIAG_STAT: %d\n", G_IBCnav_OriginData_t.DIAG_STAT);

    println("mainIMU_WX: %f\n", G_IBCnav_OriginData_t.mainIMU_WX);
    println("mainIMU_WY: %f\n", G_IBCnav_OriginData_t.mainIMU_WY);
    println("mainIMU_WZ: %f\n", G_IBCnav_OriginData_t.mainIMU_WZ);

    println("mainIMU_FX: %f\n", G_IBCnav_OriginData_t.mainIMU_FX);
    println("mainIMU_FY: %f\n", G_IBCnav_OriginData_t.mainIMU_FY);
    println("mainIMU_FZ: %f\n", G_IBCnav_OriginData_t.mainIMU_FZ);

    println("backupIMU_WX: %f\n", G_IBCnav_OriginData_t.backupIMU_WX);
    println("backupIMU_WY: %f\n", G_IBCnav_OriginData_t.backupIMU_WY);
    println("backupIMU_WZ: %f\n", G_IBCnav_OriginData_t.backupIMU_WZ);

    println("backupIMU_FX: %f\n", G_IBCnav_OriginData_t.backupIMU_FX);
    println("backupIMU_FY: %f\n", G_IBCnav_OriginData_t.backupIMU_FY);
    println("backupIMU_FZ: %f\n", G_IBCnav_OriginData_t.backupIMU_FZ);

    println("MAG_HX: %f\n", G_IBCnav_OriginData_t.MAG_HX);
    println("MAG_HY: %f\n", G_IBCnav_OriginData_t.MAG_HY);
    println("MAG_HZ: %f\n", G_IBCnav_OriginData_t.MAG_HZ);

    println("GPS_sec: %f\n", G_IBCnav_OriginData_t.GPS_sec);
    println("heading: %f\n", G_IBCnav_OriginData_t.heading);
    println("heading_type: %d\n", G_IBCnav_OriginData_t.heading_type);

    println("hor_spd: %f\n", G_IBCnav_OriginData_t.hor_spd);
    println("trk_gnd: %f\n", G_IBCnav_OriginData_t.trk_gnd);
    println("vert_spd: %f\n", G_IBCnav_OriginData_t.vert_spd);

    println("lat: %f\n", G_IBCnav_OriginData_t.lat);
    println("lon: %f\n", G_IBCnav_OriginData_t.lon);
    println("alt: %f\n", G_IBCnav_OriginData_t.hgt);

    println("pos_type: %d\n", G_IBCnav_OriginData_t.pos_type);
    println("PPS_dt:%d\n", G_IBCnav_OriginData_t.PPS_dt);

    println("checksum: %d\n", G_IBCnav_OriginData_t.checksum);
}

void IBCnav_prfFusionData(int (*println)(const char *fmt, ...))
{
    println("\n\nFusionData:\n");
    println("head: 0x%04x\n", G_IBCnav_FusionData_t.head);
    println("len: %d\n", G_IBCnav_FusionData_t.len);
    println("id: %d\n", G_IBCnav_FusionData_t.id);

    println("counter: %u\n", G_IBCnav_FusionData_t.count);
    println("state: 0x%02X\n", G_IBCnav_FusionData_t.state);

    println("pitch: %.3f rad\n", G_IBCnav_FusionData_t.pitch);
    println("roll: %.3f rad\n", G_IBCnav_FusionData_t.roll);
    println("yaw: %.3f rad\n", G_IBCnav_FusionData_t.yaw);
    println("yaw_gps: %.3f deg\n", G_IBCnav_FusionData_t.yaw_gps);

    println("pitch_rate: %.3f rad/s\n", G_IBCnav_FusionData_t.pitch_rate);
    println("roll_rate: %.6f rad/s\n", G_IBCnav_FusionData_t.roll_rate);
    println("yaw_rate: %.6f rad/s\n", G_IBCnav_FusionData_t.yaw_rate);

    println("lon: %.7lf deg\n", (double)G_IBCnav_FusionData_t.lon * 1e-7);
    println("lat: %.7lf deg\n", (double)G_IBCnav_FusionData_t.lat * 1e-7);

    println("alt_baro: %.2f m\n", (double)G_IBCnav_FusionData_t.alt_baro / 100);
    println("alt_gps: %.2f m\n", (double)G_IBCnav_FusionData_t.alt_gps / 100);
    println("alt: %.2f m\n", (double)G_IBCnav_FusionData_t.alt / 100);

    println("velocity_x: %.2f m/s\n", G_IBCnav_FusionData_t.velocity_x);
    println("velocity_y: %.2f m/s\n", G_IBCnav_FusionData_t.velocity_y);
    println("velocity_z: %.2f m/s\n", G_IBCnav_FusionData_t.velocity_z);
    println("velocity_air: %.2f m/s\n", G_IBCnav_FusionData_t.velocity_air);

    println("accel_x: %.3f m/s^2\n", G_IBCnav_FusionData_t.accel_x);
    println("accel_y: %.3f m/s^2\n", G_IBCnav_FusionData_t.accel_y);
    println("accel_z: %.3f m/s^2\n", G_IBCnav_FusionData_t.accel_z);

    println("satellite_num: %d\n", G_IBCnav_FusionData_t.satellite_num);
    println("hdop: %.2f m\n", (double)G_IBCnav_FusionData_t.hdop / 100);
    println("vdop: %.2f m\n", (double)G_IBCnav_FusionData_t.vdop / 100);

    println("gps_status: %d\n", G_IBCnav_FusionData_t.gps_status);
    println("gps_hh: %d\n", G_IBCnav_FusionData_t.gps_hh);
    println("gps_mm: %d\n", G_IBCnav_FusionData_t.gps_mm);
    println("gps_ss: %d\n", G_IBCnav_FusionData_t.gps_ss);

    println("temperature: %d degC\n", G_IBCnav_FusionData_t.temperature);
    println("HDT: %.2f deg\n", (double)G_IBCnav_FusionData_t.HDT / 10);
    println("HDG_Dev: %.2f deg\n", (double)G_IBCnav_FusionData_t.HDG_Dev / 10);

    println("redundancy: %d\n", G_IBCnav_FusionData_t.redundancy);
    println("GPS0_DT: %d ms\n", G_IBCnav_FusionData_t.GPS0_DT * 100);
    println("GPS1_DT: %d ms\n", G_IBCnav_FusionData_t.GPS1_DT * 100);

    println("GPS_vx: %.2f m/s\n", G_IBCnav_FusionData_t.GPS_vx);
    println("GPS_vy: %.2f m/s\n", G_IBCnav_FusionData_t.GPS_vy);
    println("GPS_vz: %.2f m/s\n", G_IBCnav_FusionData_t.GPS_vz);

    println("gps_ms: %u\n", G_IBCnav_FusionData_t.gps_ms);
    println("gps_day: %d\n", G_IBCnav_FusionData_t.gps_day);
    println("gps_week: %u\n", G_IBCnav_FusionData_t.gps_week);
    println("ahrs_state: %d\n", G_IBCnav_FusionData_t.ahrs_state);

#if IBCnav_ENABLE_MOBILE_PLATFORM
    println("eastward_dis: %.2f m\n", G_IBCnav_FusionData_t.eastward_dis);
    println("northward_dis: %.2f m\n", G_IBCnav_FusionData_t.northward_dis);
    println("skyward_dis: %.2f m\n", G_IBCnav_FusionData_t.skyward_dis);
    println("std_dev: %.2f m\n", (double)G_IBCnav_FusionData_t.std_dev / 100);
    println("std_dev_up: %.2f m\n", (double)G_IBCnav_FusionData_t.std_dev_up / 100);

    println("ture_north: %.2f deg\n", (double)G_IBCnav_FusionData_t.ture_north / 100);
    println("Ground_speed: %.2f m/s\n", (double)G_IBCnav_FusionData_t.Ground_speed / 100);
#endif

    println("Engine_RPM: %u rpm\n", G_IBCnav_FusionData_t.Engine_RPM);
    println("check_sum: 0x%04X\n", G_IBCnav_FusionData_t.check_sum);
}
